int menu=0;
int action=0;
int servo=0;

void show_menu_0()
{
  Serial.println("Menu");
  Serial.println("'1' - Calibration");
}

void action_menu_0(String sCmd)
{
  if(action == 0)
  {
    if(sCmd.equalsIgnoreCase("1"))
    {
      menu=1;
      action=0;
      show_menu_1();
    }
    else show_menu_0();
  }
  else action=0;
}

void show_menu_1()
{
  Serial.print("Calibration servo ");
  Serial.println(servo);

  Serial.println("'1' - change servo");
  Serial.println("'2' - set cal");
  Serial.println("'3' - show cal");

  Serial.println("'0' - back");
}

void action_menu_1(String sCmd)
{
  if(action == 0)
  {
    if(sCmd.equalsIgnoreCase("0"))
    {
      menu=0;
      action=0;
      show_menu_0();
    }
    else if(sCmd.equalsIgnoreCase("1"))
    {
      action=1;
      Serial.println("Changing current servo number");
      Serial.println("- 'x' for exit");
    }
    else if(sCmd.equalsIgnoreCase("2"))
    {
      action=2;
      Serial.println("Changing servo calibration");
      Serial.println("- 'x' for exit");
    }
    else if(sCmd.equalsIgnoreCase("3"))
    {
      action=0;
      Serial.print("servo ");
      Serial.print(servo);
      Serial.print(" calibration is ");
      Serial.println(servo_cal[1][servo]);
    }
    else show_menu_1();
    return;
  }
  else if(action==1)
  {
    action_servo(sCmd);
  }
  else if(action==2)
  {
    action_calibration(sCmd);
  }
  else action=0;
}

void action_calibration(String sCmd)
{
  if(sCmd.equalsIgnoreCase("x"))
  {
    action=0;
    show_menu_1();
    return;
  }

  int iCal = sCmd.toInt();
  servo_cal[1][servo] = iCal;
  servo_update[servo] = true;

  Serial.print("set ");
  Serial.print(iCal);
  Serial.print(" to servo ");
  Serial.println(servo);
}

void action_servo(String sCmd)
{
  if(sCmd.equalsIgnoreCase("x"))
  {
    action=0;
    show_menu_1();
    return;
  }

  int toServo = sCmd.toInt();
  if(toServo<0 || servo>N_SERVOS)
  {
    Serial.print("bad parameter !");
    return;
  }
 
  servo=toServo;
  Serial.print("set current servo to ");
  Serial.println(servo);
  action=0;
  show_menu_1();
}

boolean check_basic_commands(void)
{
  String line=Serial.readStringUntil('\n');
  line.trim();
  if(line.length()==0) return false;
  if(menu==0)
  {
    action_menu_0(line);
  }
  else if(menu==1)
  {
    action_menu_1(line);
  }

  return true;
}





